#include "config.h"


#define MR_UP	1
#define MR_DN	2

enum
{
	IS_PWON=0,
	IS_WAITE,
	IS_BL,
	IS_TEST,
}State = IS_PWON;

u8 Step = CMD_IDLE;

u8 mRun = 0;
u16 adc_buf[20],adc_n=0;
u16 MotorI_ma=0;
u32 RunTime_ms = 0;;
struct
{
    u8 h1:1;
    u8 h2:1;
    u8 th1:3;
    u8 th2:3;
}iHall;
u16 mPos = 0, stopPos = 0;
u16 tH_01ms = 0;
u16 TH_buf[4], TH_n=0;
MOTOR_SPEEDs_def mSpd;
u8 mEer = 0;
int16 Pwm;

u8 rf433_Buf[5],RfData[7],Rf_isOk=0,Rf_isEn=0;
u16 wakeN = 0;
u32 RssiBuf[10];
u32 RssiValue[20];
u16 vRssi_cur,vRssi;

u8 ack[80];
u8 acksize=0;

u8 isChkKey = 0;

static u16 get_3v3(void)
{
	u32 sum = 0 ,re;
	u8 i;
	M0P_ADC->CR0_f.BUF = 1u;
	for(i=0;i<10;i++)
	{
		sum += Get_Adc(AD1V2_CH);
		Delay_nMs(2);
	}
	M0P_ADC->CR0_f.BUF = 0u;
	re = 49152000/sum;
	return re;
}

static u16 get_8v(void)
{
	u32 sum = 0 ,re;
	u8 i;
	VBAT_ON();
	for(i=0;i<10;i++)
	{
		sum += Get_Adc(ADVBAT_CH);
		Delay_nMs(2);
	}
	re = sum*325/1000;
	return re;
}

static u16 get_5v(void)
{
	u32 sum = 0 ,re;
	u8 i;
	for(i=0;i<10;i++)
	{
		sum += Get_Adc(ADV5_CH);
		Delay_nMs(2);
	}
	re = sum*325/1000;
	return re;
}

static u16 get_vRssi(void)
{
	u32 rssi,re;
	rssi = Get_Adc(RSSI_CH);
	re = rssi*8/10;
	return re;
}

static void Led_Notice(void)
{
	static u8 tled=0;
	tled ++;
	if(tled ==1)
		LED1ON();
	else
		LED1OFF();

	if(tled > 20)
		tled = 0;
}

static void Pwon_Jump(void)
{
	u8 chk = Check_TestCode();
    if(chk&0x1)
        sPrint("TestPcba OK,");
    else
        sPrint("TestPcba NG,");
    if(chk&0x2)
        sPrint("TestDev1 OK,");
    else
        sPrint("TestDev1 NG,");
    if(chk&0x4)
        sPrint("TestDev2 OK,");
    else
        sPrint("TestDev2 NG,");
    Delay_nMs(50);
	if((chk == 0x05)||(chk == 0x07))
		iap_load_app(APP_START_ADDR);
}

static void check_rssi(void)
{
	u8 i;
	u32 delay;
	u32 r = 0;
	wakeN = 0;
	while(1)
	{
		__WFI();
		Wdt_Start();
		
		M0P_BGR->CR_f.BGR_EN = 1u;
	    M0P_ADC->CR0_f.EN = 1u;
	    CSRF_ON();
	    CSOP_ON();
	    for(i=0;i<5;i++)
	    {
	    	RssiBuf[i] = Get_Adc_4M(RSSI_CH);
	    }
	    CSRF_OFF();
	    CSOP_OFF();
	    delay = 300;
	    while(delay--);
	    CSRF_ON();
	    CSOP_ON();
	    for(i=5;i<10;i++)
	    {
	    	RssiBuf[i] = Get_Adc_4M(RSSI_CH);
	   	}
	    CSRF_OFF();
	    CSOP_OFF();
		M0P_BGR->CR_f.BGR_EN = 0u;
	    M0P_ADC->CR0_f.EN = 0u; 
		r = 0;
	    for(i=1; i<10; i++)
	    {
	    	if(i == 5)
	    		continue;
	    	r += RssiBuf[i];
	   	}

	   	RssiValue[wakeN%20] = r;
	   	wakeN ++;
	   	if(wakeN >= 32)
	   		break;
	}
}

static void check_dsleep(void)
{
	wakeN = 0;
	while(1)
	{
		__WFI();
		Wdt_Start();
		wakeN ++;
	   	if(wakeN >= 32)
	   		break;
	}
}

static void wait_sleep(void)	// sleep 1s
{
	wakeN = 0;
	while(1)
	{
		__WFI();
		Wdt_Start();
		wakeN ++;
	   	if(wakeN >= 16)
	   		break;
	}
}

static void in_sleep(void)
{
	Wdt_Start();
	DnPWM_SET(1001);
    UpPWM_SET(1001);
	BREAK_ON();
	VBAT_OFF();
	VHALL_OFF();
	CSOP_OFF();
	CSRF_OFF();
	LED1OFF();   
	M0P_ADC->CR0_f.EN = 0u;
	M0P_BGR->CR_f.BGR_EN = 0u;
	
	Gpio_ClearIrq(KEY_PORT, KEY_PIN);
	Gpio_EnableIrq(KEY_PORT, KEY_PIN, GpioIrqFalling);
	
	Gpio_DisableIrq(RF_PORT, RF_PIN, GpioIrqFalling);
	Gpio_DisableIrq(RF_PORT, RF_PIN, GpioIrqRising);

	SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;

	__disable_irq();
    Set_Hclk_4M();
    __enable_irq();
}

static void out_sleep(void)
{
	__disable_irq();
    Set_Hclk_16M();
    __enable_irq();
	M0P_ADC->CR0_f.EN = 1u;
	M0P_BGR->CR_f.BGR_EN = 1u;
	Gpio_DisableIrq(KEY_PORT, KEY_PIN, GpioIrqFalling);
}

static void Check_State_10ms(void)
{
	static u8 tJump = 0;
	switch(State)
	{
		case IS_PWON:
		case IS_WAITE:
			if(Tick.outtime_1ms>100)
			{
				Tick.outtime_1ms = 0;
				//in_sleep();
				//wait_sleep();
				//out_sleep();
				tJump ++;
				//if(tJump >= 5)
				if(tJump >= 100)
				{
					tJump  = 0;
					Pwon_Jump();
					State = IS_WAITE;
				}
			}
			
			if(Tick.outtime_1ms<50)
				LED1ON();
			else
				LED1OFF();
			break;
		case IS_BL:
			Led_Notice();
			if(Tick.outtime_1ms>2000)
			{
				Tick.outtime_1ms = 0;
				State = IS_WAITE;
			}
			break;
		case IS_TEST:
			if(Tick.outtime_1ms>60000)
			{
				Tick.outtime_1ms = 0;
				isChkKey = 0;
				State = IS_WAITE;
				DnPWM_SET(1001);
			    UpPWM_SET(1001);
				BREAK_ON();
				VBAT_OFF();
			    VHALL_OFF();
			    CSOP_OFF();
			 	CSRF_OFF();
			}
			break;
	}
}


static void mbreak(void)
{
	BREAK_ON();
	Delay_nMs(7);
	BREAK_OFF();
	Delay_nMs(7);
	BREAK_ON();
	Delay_nMs(7);
	BREAK_OFF();
	Delay_nMs(7);
	BREAK_ON();
}

static void Stop(void)
{   
    DnPWM_SET(0);
    UpPWM_SET(0);

    Delay_nMs(5);
    mbreak();
    mRun = 0;
    stopPos = mPos;
}

static void motor_run_init(void)
{  
    DnPWM_SET(0);
    UpPWM_SET(0);

    BREAK_OFF();
	tH_01ms = 0;
	mEer = 0;
	Tick.outtime_1ms = 0;
	mPos = POS_INIT;

	Pwm = PWM_INIT;
	mSpd.thall = SPD_PROID_INIT;
    mSpd.proid = SPD_PROID_INIT;
    mSpd.set = SPD2_PROID;
}

static void m_up(void)
{   
	mRun = MR_UP;
    motor_run_init();
    DnPWM_SET(Pwm);
}

static void m_dn(void)
{
	mRun = MR_DN;
    motor_run_init();
    UpPWM_SET(Pwm);
}

void Read_Key_10ms(void)
{
    static u8 t=0, skey = 0;
    u8 cur;
    u8 buf[1];
    
    if(R_KEY() == 0)
        cur = 1;
    else
        cur = 0;
    
    if(skey == cur)
        t = 0;
    else
        t ++;

    if(t >= 5)
    {
        if(cur)
        {
            LED1TOG();
            if(isChkKey)
            {
                buf[0] = 1;
                Comm_Ack(Step, 1, buf);
            }
        }
        skey = cur;
    }
}

void Read_Pw_10ms(void)
{
    static u8 t=0, skey = 0;
    u8 cur;
    u8 buf[1];
    
    if(R_LPMOD() == 1)
        cur = 1;
    else
        cur = 0;
    
    if(skey == cur)
        t = 0;
    else
        t ++;

    if(t >= 5)
    {
        LED1TOG();
        if(isChkKey)
        {
            if(cur)
                buf[0] = 3;
            else
                buf[0] = 2;
            Comm_Ack(Step, 1, buf);
        }
        skey = cur;
    }
}


void Scan_Hall_Handler(void)
{
	u8 c_h1,c_h2;
	u8 isNew = 0;

	tH_01ms ++;
	mSpd.thall ++;
	
	
	c_h1 = R_HALL1();
	c_h2 = R_HALL2();
	
	if(c_h1 == iHall.h1)
		iHall.th1 = 0;
	else
	{
		iHall.th1 ++;
		if(iHall.th1>2)
		{
			iHall.th1 = 0;
			iHall.h1 = c_h1;
			isNew = 1;
			if(iHall.h1)
			{
				mSpd.proid = mSpd.thall;
				mSpd.thall = 0;
				mSpd.isNew = 1;
			}
		}
	}	
	
	if(c_h2 == iHall.h2)
		iHall.th2 = 0;
	else
	{
		iHall.th2 ++;
		if(iHall.th2>2)
		{
			iHall.th1 = 0;
			iHall.h2 = c_h2;
			isNew = 1;
			if(iHall.h1 == iHall.h2)
				mPos ++;
			else
				mPos --;
		}
	}

	if(isNew)
	{
		TH_buf[TH_n] = tH_01ms;	
		tH_01ms = 0;	
		TH_n ++;
		if(TH_n >= 4)
			TH_n = 0;
	}
}


void Get_MotorRunStop_1ms(void)
{
	u8 buf[10];
	if(mRun == 0)
		return;
	if(Tick.outtime_1ms<RunTime_ms)
		return;
	buf[0] = MotorI_ma&0xff;
	buf[1] = (MotorI_ma>>8)&0xff;
	buf[2] = mPos&0xff;
	buf[3] = (mPos>>8)&0xff;
	Stop();
	Comm_Ack(Step, 4, buf);
}

void Get_MotorRunI_10ms(void)
{
	u32 sum = 0;
	u8 i;
	if(mRun == 0)
		return;
	adc_buf[adc_n] = Get_Adc(ADMI_CH);
	adc_n ++;
	if(adc_n>=20)
	{
		adc_n = 0;
		for(i=0;i<20;i++)
			sum += adc_buf[i];
		MotorI_ma = sum*17/200;
	}
}

void Get_MotorRunHall_100ms(void)
{
	u16 th,th_max,th_min;
	u8 buf[10];
	if(mRun == 0)
		return;

	if((mPos>(POS_INIT+20))||(mPos<(POS_INIT-20)))
	{
		th = (TH_buf[0]+TH_buf[1]+TH_buf[2]+TH_buf[3])/4;
		th_max = th + th/2;
		th_min = th - th/2;
		if((TH_buf[0]>th_max)||(TH_buf[0]<th_min))
			mEer = MEER_THALL;
		if((TH_buf[1]>th_max)||(TH_buf[1]<th_min))
			mEer = MEER_THALL;
		if((TH_buf[2]>th_max)||(TH_buf[2]<th_min))
			mEer = MEER_THALL;
		if((TH_buf[3]>th_max)||(TH_buf[3]<th_min))
			mEer = MEER_THALL;
	}
	if(mEer)
	{
		buf[0] = MotorI_ma&0xff;
		buf[1] = (MotorI_ma>>8)&0xff;
		buf[2] = mPos&0xff;
		buf[3] = (mPos>>8)&0xff;
		Stop();
		Comm_Ack(Step, 4, buf);
	}
	
}

void CkeckHall_10ms(void)
{
	u8 buf[10];
    if(mRun == 0)
        return;
        
    if(mSpd.thall>=MTHALL_MAX)
    {
    	mSpd.proid = SPD_PROID_INIT;
    	
    	mEer = MEER_NOHALL;
    	buf[0] = MotorI_ma&0xff;
		buf[1] = (MotorI_ma>>8)&0xff;
		buf[2] = mPos&0xff;
		buf[3] = (mPos>>8)&0xff;
		Stop();
		Comm_Ack(Step, 4, buf);
        return;
    }
	if(mSpd.thall>200)
	{
		Pwm += PWM_NOHALL_X;
		if(Pwm>PWM_INIT)
        	Pwm = PWM_INIT;
	    if(mRun == MR_UP)
	        DnPWM_SET(Pwm);
	    else
	        UpPWM_SET(Pwm);
	}
}

void CtrlSpdx_tHandle(void)
{
	int x;
	
	if(mRun==0)
        return;
	if(mSpd.tPid < 20)
		return;
	mSpd.tPid = 0;
	
    if(mSpd.proid >= SPD_PROID_INIT)
    	return;

    //LED1TOG();

	x = mSpd.proid - mSpd.set;
	if(x<-25)
		x = x*2;
	if((x>10)||(x<-10))
		x=x;
	else
		x = x/2;
	
    if(x>PWM_X)
        x = PWM_X;
    else if(x<(-PWM_X))
        x = -PWM_X;
    Pwm += x;
    if(Pwm>PWM_MAX)
        Pwm = PWM_MAX;
    else if(Pwm<PWM_MIN)
        Pwm = PWM_MIN;
    if(mRun == MR_UP)
        DnPWM_SET(Pwm);
    else
        UpPWM_SET(Pwm);
}

void Get_rf433_data_1ms(void)
{
	if(Rf_isEn == 0)
		return;
	vRssi_cur = get_vRssi();
	if(Tick.outtime_1ms>2000)
	{
		Rf_isEn = 0;
		Gpio_DisableIrq(RF_PORT, RF_PIN, GpioIrqFalling);
		Gpio_DisableIrq(RF_PORT, RF_PIN, GpioIrqRising);
	}
	if(Rf_isOk == 0)
		return;
	Rf_isOk = 0;
	if(RfData[4] == 0x05)
	{
		RfData[5] = vRssi&0xff;
		RfData[6] = (vRssi>>8)&0xff;
		Comm_Ack(Step, 7, RfData);
	}
}

void BL_Receive(u8 cmd, u8 size, u8 *buf)
{
	u8 ack[64];
	u8 acksize=0;
	u16 addr;
	u32 chksum0,chksum1;
	switch(cmd)
	{
		case CMD_CONNECT_BL:
			if(	(buf[0] == KEY_BL_buf[0])&&
			    (buf[1] == KEY_BL_buf[1])&&
			    (buf[2] == KEY_BL_buf[2])&&
			    (buf[3] == KEY_BL_buf[3]))
			{
			    State = IS_BL;
			    ack[0] = 0x01;
			    Comm_Ack(cmd, 1, ack);
			}
			return;
		case CMD_CONNECT_APP:
		case CMD_GOTO_APP:
			if(	(buf[0] == KEY_APP_buf[0])&&
			    (buf[1] == KEY_APP_buf[1])&&
			    (buf[2] == KEY_APP_buf[2])&&
			    (buf[3] == KEY_APP_buf[3]))
			{
				iap_load_app(APP_START_ADDR);
				while(1);
			}
			return;
		case CMD_RESET:
			if(	(buf[0] == KEY_BL_buf[0])&&
			    (buf[1] == KEY_BL_buf[1])&&
			    (buf[2] == KEY_BL_buf[2])&&
			    (buf[3] == KEY_BL_buf[3]))
			{
			    Soft_Reset();
			}
			return;
		case CMD_BL_FLASHINFO:
			acksize = FLASH_INFO_SIZE;
			Get_FlashInfo(BL_INFO_ADDR, ack);
			Comm_Ack(cmd, acksize, ack);
			return;
		case CMD_APP_FLASHINFO:
			acksize = FLASH_INFO_SIZE;
			Get_FlashInfo(APP_INFO_ADDR, ack);
			Comm_Ack(cmd, acksize, ack);
			return;
		case CMD_TESTCODE:
			Get_TestCode(ack);
			ack[6] = Check_TestCode();
			Comm_Ack(cmd, 7, ack);
			return;
		case CMD_WRITE_TestCode:
			if(Write_TestCode(1)==0)
				ack[0] = 0;
			if(Write_TestCode(2)==0)
				ack[0] = 0;
			if(Write_TestCode(4)==0)
				ack[0] = 0;
			Comm_Ack(cmd, 1, ack);
			return;
		case CMD_GET_UID:
			Get_UID(ack);
			Comm_Ack(cmd, 10, ack);
			return;
		case CMD_READFLASH:
			addr = buf[0] + (buf[1]*256);
			acksize = buf[2];
			if(acksize > 64)
				acksize = 64;
			DFlash_ReadBytes(addr, ack, acksize);
			Comm_Ack(cmd, acksize, ack);
			return;
	}

	if(State != IS_BL)
		return;

	Tick.outtime_1ms = 0;
		
	switch(cmd)
	{
		case UD_WRITE_BL:
			;
			break;
		case UD_WRITE_BLEND:
			;
			break;
		case UD_WRITE_APP:
			acksize = 1;
			addr = buf[0] + (buf[1]*256);
            if(addr == 0)
            {
                ack[0] = DFlash_ErasePage(APP_START_ADDR, APP_END_ADDR);
                if(ack[0] == 0)
                	break;
            }
            ack[0] = DFlash_WriteBytes(addr+APP_START_ADDR, buf+2, 32);
            Comm_Ack(cmd, 1, ack);
			break;
		case UD_WRITE_APPEND:
			addr = buf[0] + (buf[1]*0x100);
			chksum0 = DFlash_CheckSum(APP_START_ADDR, APP_START_ADDR+addr);
            chksum1 = buf[2] + (buf[3]*0x100) + (buf[4]*0x10000) + (buf[5]*0x1000000);
            if(chksum0 == chksum1)
            	ack[0] = 1;
            else
            	ack[0] = 0;
            acksize = 1;
            if(ack[0] == 1)
            {
				Comm_Ack_NoInt(cmd, 1, ack);
				Delay_nMs(50);
				iap_load_app(APP_START_ADDR);
			    while(1);
            }
            else
            	Comm_Ack(cmd, 1, ack);
			break;
		default:
			ack[0] = 0;
            acksize = 1;
            Comm_Ack(cmd, 1, ack);
	}
}

void Test_Receive(u8 cmd, u8 dsize, u8 *data)
{
	u16 v1,v2;
	
	if(cmd == CMD_CONNECT_TEST)
	{
		if(	(data[0] == KEY_TEST_buf[0])&&
		    (data[1] == KEY_TEST_buf[1])&&
		    (data[2] == KEY_TEST_buf[2])&&
		    (data[3] == KEY_TEST_buf[3]))
		{
		    Get_UID(ack);
		    ack[10] = Check_TestCode();
		    ack[11] = VERSION;
		    Comm_Ack(cmd, 12, ack);
			State = IS_TEST;
			LED1OFF();
			VBAT_ON();
		    BREAK_ON();
		    VHALL_ON();
		    CSOP_ON();
		    CSRF_ON();
		}
		return;
	}

	if(State != IS_TEST)
		return;
	Tick.outtime_1ms = 0;
	Step = 0;
	switch(cmd)
    {
    	case tk_end:
			ack[0] = Write_TestCode(data[0]);
			Comm_Ack_NoInt(cmd, 1, ack);
			Soft_Reset();
			break;
    	case tk_vDev:
			v1 = get_3v3();
			v2 = get_8v();
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			ack[2] = v2 & 0xff;
			ack[3] = (v2>>8) & 0xff;
			Comm_Ack(cmd, 4, ack);
			break;
		case tk_iDev:
			if(data[0] == 0)
			{
				VBAT_OFF();
			    VHALL_OFF();
			    CSOP_OFF();
			    CSRF_OFF(); 
			}
			else if(data[0] == 1)
			{
				VBAT_ON();
			    VHALL_OFF();
			    CSOP_OFF();
			    CSRF_OFF();
			}
			else if(data[0] == 2)
			{
				VBAT_ON();
			    VHALL_ON();
			    CSOP_OFF();
			    CSRF_OFF();
			}
			else if(data[0] == 3)
			{
				VBAT_ON();
			    VHALL_ON();
			    CSOP_ON();
			    CSRF_ON();
			}
			Delay_nMs(50);
			ack[0] = data[0];
			Comm_Ack(cmd, 1, ack);
			break;
		case tk_io:
			if((data[0] == 0)||(data[0] == 1))
				ack[0] = R_KEY();
			else if((data[0] == 2)||(data[0] == 3))
				ack[0] = R_LPMOD();
			else if(data[0] == 4)
			{
				ack[0] = data[0];
				LED1ON();
			}
			else if(data[0] == 5)
			{
				ack[0] = data[0];
				LED1OFF();
			}
			else
				break;
			Comm_Ack(cmd, 1, ack);
			break;
		case tk_mup:
			Step = cmd;
			m_up();
			RunTime_ms = data[0] + data[1]*256;
			break;
		case tk_mdn:
			Step = cmd;
			m_dn();
			RunTime_ms = data[0] + data[1]*256;
			break;
		case tk_mups:
		case tk_mdns:
			if(mPos > stopPos)
				v1 = mPos - stopPos;
			else
				v1 = stopPos - mPos;
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			Comm_Ack(cmd, 2, ack);
			break;
		case tk_rf:
			Step = cmd;
			Rf_isEn = 1;
			Tick.outtime_1ms = 0;
			CSOP_ON();
			CSRF_ON();
			Gpio_ClearIrq(RF_PORT, RF_PIN);
		    Gpio_EnableIrq(RF_PORT, RF_PIN, GpioIrqFalling);
		    Gpio_EnableIrq(RF_PORT, RF_PIN, GpioIrqRising);
			break;
		case tk_dsleep:
			in_sleep();
			check_dsleep();
			out_sleep();
			ack[0] = 0x01;
			Comm_Ack(cmd, 1, ack);
			break;
		case tk_chg:
		case tk_chgload:
			v1 = get_5v();
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			v1 = get_8v();
			ack[2] = v1 & 0xff;
			ack[3] = (v1>>8) & 0xff;
			Comm_Ack(cmd, 4, ack);
			break;
		case tk_vChg:
			v1 = get_5v();
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			Comm_Ack(cmd, 2, ack);
			break;
		case tk_mio:
			Step = cmd;
			isChkKey = 1;
			LED1ON();
			break;
			
    	case TEST_CHK_VERSION:
    		Get_FlashInfo(BL_INFO_ADDR, ack);
    		acksize = FLASH_INFO_SIZE;
    		Comm_Ack(cmd, acksize, ack);
    		break;
		case TEST_KEY_Hi:
		case TEST_KEY_Lo:
			ack[0] = R_KEY();
			Comm_Ack(cmd, 1, ack);
			break;
		case TEST_PW_Hi:
		case TEST_PW_Lo:
			ack[0] = R_LPMOD();
			Comm_Ack(cmd, 1, ack);
			break;
		case TEST_LED_Hi:
			LED1ON();
			Comm_Ack(cmd, 1, ack);
			break;
		case TEST_LED_Lo:
			LED1OFF();
			Comm_Ack(cmd, 1, ack);
			break;
		case TEST_IO_M:
			Step = cmd;
			isChkKey = 1;
			LED1ON();
			break;
		case TEST_PW33:
			v1 = get_3v3();
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			Comm_Ack(cmd, 2, ack);
			break;
		case TEST_VBAT:
		case TEST_VDEV:
			v1 = get_8v();
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			Comm_Ack(cmd, 2, ack);
			break;
		case TEST_VUSB:
			v1 = get_5v();
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			Comm_Ack(cmd, 2, ack);
			break;
		case TEST_STANDBY_I:
			VBAT_ON();
		    VHALL_ON();
		    CSOP_OFF();
		    CSRF_OFF();
		    Delay_nMs(50);
		    Comm_Ack(cmd, 0, ack);
			break;
		case TEST_STANDBY1_I:
			VBAT_OFF();
		    VHALL_OFF();
		    CSOP_OFF();
		    CSRF_OFF();
		    Delay_nMs(50);
		    Comm_Ack(cmd, 0, ack);
			break;
		case TEST_STANDBY2_I:
			VBAT_ON();
		    VHALL_OFF();
		    CSOP_OFF();
		    CSRF_OFF();
		    Delay_nMs(50);
		    Comm_Ack(cmd, 0, ack);
			break;
		case TEST_STANDBY3_I:
			VBAT_ON();
		    VHALL_ON();
		    CSOP_OFF();
		    CSRF_OFF();
		    Delay_nMs(50);
		    Comm_Ack(cmd, 0, ack);
			break;
		case TEST_STANDBY4_I:
			VBAT_ON();
		    VHALL_ON();
		    CSOP_ON();
		    CSRF_ON();
		    Delay_nMs(50);
		    Comm_Ack(cmd, 0, ack);
			break;
		case TEST_MUP1:
		case TEST_MUP2:
		case TEST_MUP3:
			Comm_Ack(cmd, 0, ack);
			Step = cmd;
			m_up();
			RunTime_ms = data[0] + data[1]*256;
			break;
		case TEST_MDN1:
		case TEST_MDN2:
		case TEST_MDN3:
			Comm_Ack(cmd, 0, ack);
			Step = cmd;
			m_dn();
			RunTime_ms = data[0] + data[1]*256;
			break;
		case TEST_MUP_SLID:
		case TEST_MDN_SLID:
			if(mPos > stopPos)
				v1 = mPos - stopPos;
			else
				v1 = stopPos - mPos;
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			Comm_Ack(cmd, 2, ack);
			break;
		case TEST_SLEEP2s:
			in_sleep();
			check_rssi();
			out_sleep();
			Comm_Ack(cmd, 80, (u8 *)RssiValue);
			break;
		case TEST_DSLEEP2s:
			in_sleep();
			check_dsleep();
			out_sleep();
			ack[0] = 0x01;
			Comm_Ack(cmd, 1, ack);
			break;
		case TEST_RF1:
		case TEST_RF2:
		case TEST_RF3:
			Step = cmd;
			Rf_isEn = 1;
			Tick.outtime_1ms = 0;
			CSOP_ON();
			CSRF_ON();
			Gpio_ClearIrq(RF_PORT, RF_PIN);
		    Gpio_EnableIrq(RF_PORT, RF_PIN, GpioIrqFalling);
		    Gpio_EnableIrq(RF_PORT, RF_PIN, GpioIrqRising);
			break;
		case TEST_CHG:
		case TEST_CHGLOAD:
			v1 = get_5v();
			ack[0] = v1 & 0xff;
			ack[1] = (v1>>8) & 0xff;
			v1 = get_8v();
			ack[2] = v1 & 0xff;
			ack[3] = (v1>>8) & 0xff;
			Comm_Ack(cmd, 4, ack);
			break;
		case TEST_END1:
			ack[0] = Write_TestCode(1);
			Comm_Ack_NoInt(cmd, 1, ack);
			Soft_Reset();
			break;
		case TEST_END2:
			ack[0] = Write_TestCode(2);
			Comm_Ack_NoInt(cmd, 1, ack);
			Soft_Reset();
			break;
		case TEST_END3:
			ack[0] = Write_TestCode(3);
			Comm_Ack_NoInt(cmd, 1, ack);
			Soft_Reset();
			break;
		default:
			break;
	}
}


static tick_Pro(void)
{
	if(Tick.t1ms)
	{
		Tick.t1ms = 0;
		Get_MotorRunStop_1ms();
		Get_rf433_data_1ms();
		CtrlSpdx_tHandle();
	}
	
	if(Tick.t10ms >= 10)
	{
		Tick.t10ms = 0;
		Get_MotorRunI_10ms();
		CkeckHall_10ms();
		Read_Key_10ms();
		Read_Pw_10ms();
		Check_State_10ms();
	}

	if(Tick.t100ms >= 100)
	{
		Tick.t100ms = 0;
		Get_MotorRunHall_100ms();
		
		Wdt_Start();
	}

	if(Tick.t1s >= 1000)
	{
		Tick.t1s = 0;

		if(Tick.outtime_1ms>60000)
		{
			Soft_Reset();
		}
	}
}

void Test_Process(void)
{
	while(1)
	{
		tick_Pro();
		Uart_Process();
	}
}

void Rf433_Handler(void)
{
    static u8 bit_count = 0, start = 0;
    u8 nbyte,nbit;
    u32 t = 0;
    u8 i;

    t = Bt_M0_Cnt32Get(RFTIM);
    if(t<150)//0.2ms
        return;
    Bt_M0_Cnt32Set(RFTIM, 0);
    if(t>1000)//1.0ms
    {
        bit_count = 0;
        if((t>2500)&&(R_RFIO()==0))
        {
        	vRssi = vRssi_cur;
			start = 1;
		}
		return;
    }
    if((R_RFIO()==0) && start)
    {
        nbyte = bit_count>>3;
        nbit = 7 - (bit_count&0x7);
        if(t>500)
            rf433_Buf[nbyte] |= 1<<nbit;
        else
            rf433_Buf[nbyte] &= ~(1<<nbit);
        bit_count ++;
        if(bit_count>=40)
        {
            bit_count = 0;
            start = 0;
            Rf_isOk = 1;
            for(i=0;i<5;i++)
                RfData[i] = rf433_Buf[i];
        }
    }
}

void Gpio_IRQHandler(uint8_t u8Param)
{
    if(0 == u8Param) //PORTA
    {
        if(TRUE == Gpio_GetIrqStatus(RF_PORT, RF_PIN))
        {
            Rf433_Handler();
            Gpio_ClearIrq(RF_PORT, RF_PIN);
        }
    }
    if(1 == u8Param) //PORTB
    {
        M0P_GPIO->PB_ICLR = 0;
    }
    if(2 == u8Param) //PORTC
    {
    	if(TRUE == Gpio_GetIrqStatus(KEY_PORT, KEY_PIN))
        {
            //WakupType = WK_KEY;
        }
        M0P_GPIO->PC_ICLR = 0;
    }
    if(3 == u8Param) //PORTD
    {
        M0P_GPIO->PD_ICLR = 0;
    }
}

void WdtCallback(void)
{
	Wdt_Start();
}


